
#include <warper.h>

#define  LOG_TAG    "warper"
#define  LOGI(...)  __android_log_print(ANDROID_LOG_INFO,LOG_TAG,__VA_ARGS__)
#define  LOGE(...)  __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__)

typedef struct {
	uint8_t red;
	uint8_t green;
	uint8_t blue;
	uint8_t alpha;
} argb;

float Distance(float a, float b, float c, float d)
{
	return (float)sqrt((a-c)*(a-c) + (b-d)*(b-d));
}
float Distance_toline(float x, float y, float a, float b, float c, float d)
{
	return abs(a * x + b * y + c) / d;
}
uint8_t LinearInterpolation(uint8_t a, uint8_t b, float fraction)
{
	uint8_t rs = 0;
	rs = (uint8_t)(fraction * a + (1 - fraction) * b);
	return rs;
}
int ImageSmoothApply(void **pointerpixelscolor, Point *effected, FloatPoint *fp, int n, int w, int h, int stride)
{
	void* pixelscolor = *pointerpixelscolor;
	argb *temp = (argb *)malloc(w*h*sizeof(argb));
	int x, y;
	// clone pixelcolor into a array
	argb * line = (argb *) pixelscolor;
	for (y=0;y<h;y++)
	{
	    for (x=0;x<w;x++)
	    {
	    	int id = y * w + x;
	    	temp[id].red = line[id].red;
	    	temp[id].green = line[id].green;
	    	temp[id].blue = line[id].blue;
	    }
	}

	//===========================================================================================
	// smooth
	double xOffset, yOffset;
	//LOGI("step 1");
	double fraction_x, fraction_y, one_minus_x, one_minus_y;
	int ceil_x, ceil_y, floor_x, floor_y;
	int p1, p2;
	//LOGI("w = %d, h = %d, stride = %d",w, h, stride);
	y = 0;
	//LOGI("%d %d", y, h);
	int i;
	for(i=0; i<n;i++)
	{
		x = effected[i].X;
		y = effected[i].Y;
		xOffset = fp[i].X;
		yOffset = fp[i].Y;

		// Setup
		floor_x = (int)(xOffset);
		floor_y = (int)(yOffset);
		ceil_x = (int)(xOffset + 0.5);
		ceil_y = (int)(yOffset +0.5);
		fraction_x = xOffset - floor_x;
		fraction_y = yOffset - floor_y;
		one_minus_x = 1.0 - fraction_x;
		one_minus_y = 1.0 - fraction_y;
		int id = y * w + x;
		int temp_id = floor_y * w + floor_x;

		if (floor_y >= 0 && ceil_y < h && floor_x >= 0 && ceil_x < w)
		{
			// Blue
			//LOGI("old blue %d", ((argb*)pixelscolor)[id].blue);
			p1 = LinearInterpolation(temp[floor_y * w + floor_x].blue,  temp[floor_y * w + ceil_x].blue, 1- fraction_x);
			p2 = LinearInterpolation(temp[ceil_y * w + floor_x].blue, temp[ceil_y * w + ceil_x].blue, 1-fraction_x);
			((argb*)pixelscolor)[id].blue = LinearInterpolation(p1,p2,one_minus_y);
			//LOGI("new blue %d", ((argb*)pixelscolor)[id].blue);

			// Green
			p1 = LinearInterpolation(temp[floor_y * w + floor_x].green,  temp[floor_y * w + ceil_x].green, 1- fraction_x);
			p2 = LinearInterpolation(temp[ceil_y * w + floor_x].green, temp[ceil_y * w + ceil_x].green, 1-fraction_x);
			((argb*)pixelscolor)[id].green = LinearInterpolation(p1,p2,one_minus_y);

			// Red
			p1 = LinearInterpolation(temp[floor_y * w + floor_x].red,  temp[floor_y * w + ceil_x].red, 1- fraction_x);
			p2 = LinearInterpolation(temp[ceil_y * w + floor_x].red, temp[ceil_y * w + ceil_x].red, 1-fraction_x);
			((argb*)pixelscolor)[id].red = LinearInterpolation(p1,p2,one_minus_y);
		}
	}
	//LOGI("smooth xong");
	free(temp);
	return 0;
}
int ImageSmoothApply2(void **pointerpixelscolor, FloatPoint *fp, int w, int h, int stride)
{
	void* pixelscolor = *pointerpixelscolor;
	argb *temp = (argb *)malloc(w*h*sizeof(argb));
	int x, y;
	// clone pixelcolor into a array
	argb * line = (argb *) pixelscolor;
	for (y=0;y<h;y++) {
	    	for (x=0;x<w;x++) {
	    		int id = y * w + x;
	    		temp[id].red = line[id].red;
	    		temp[id].green = line[id].green;
	    		temp[id].blue = line[id].blue;
	    	}
	    }

	//===========================================================================================
	// smooth
	double xOffset, yOffset;
	//LOGI("step 1");
	double fraction_x, fraction_y, one_minus_x, one_minus_y;
	int ceil_x, ceil_y, floor_x, floor_y;
	int p1, p2;
	//LOGI("w = %d, h = %d, stride = %d",w, h, stride);
	y = 0;
	//LOGI("%d %d", y, h);
	for(y = 0; y < h; y++)
	{
		for(x = 0; x < w; x++ )
		{
			int offsetid = y * w + x;
			xOffset = fp[offsetid].X;
			yOffset = fp[offsetid].Y;
			if(x == xOffset && y == yOffset)
				continue;
			//LOGI("x = %d, y = %d, x = %8.3f , y = %8.3f", x, y, fp[offsetid].X, fp[offsetid].Y);

			// Setup
			floor_x = (int)(xOffset);
			floor_y = (int)(yOffset);
			ceil_x = (int)(xOffset + 0.5);
			ceil_y = (int)(yOffset +0.5);
			fraction_x = xOffset - floor_x;
			fraction_y = yOffset - floor_y;
			one_minus_x = 1.0 - fraction_x;
			one_minus_y = 1.0 - fraction_y;
			int id = y * w + x;
			int temp_id = floor_y * w + floor_x;

			if (floor_y >= 0 && ceil_y < h && floor_x >= 0 && ceil_x < w)
			{
				// Blue
				//LOGI("old blue %d", ((argb*)pixelscolor)[id].blue);
				p1 = LinearInterpolation(temp[floor_y * w + floor_x].blue,  temp[floor_y * w + ceil_x].blue, 1- fraction_x);
				p2 = LinearInterpolation(temp[ceil_y * w + floor_x].blue, temp[ceil_y * w + ceil_x].blue, 1-fraction_x);
				((argb*)pixelscolor)[id].blue = LinearInterpolation(p1,p2,one_minus_y);
				//LOGI("new blue %d", ((argb*)pixelscolor)[id].blue);

				// Green
				p1 = LinearInterpolation(temp[floor_y * w + floor_x].green,  temp[floor_y * w + ceil_x].green, 1- fraction_x);
				p2 = LinearInterpolation(temp[ceil_y * w + floor_x].green, temp[ceil_y * w + ceil_x].green, 1-fraction_x);
				((argb*)pixelscolor)[id].green = LinearInterpolation(p1,p2,one_minus_y);

				// Red
				p1 = LinearInterpolation(temp[floor_y * w + floor_x].red,  temp[floor_y * w + ceil_x].red, 1- fraction_x);
				p2 = LinearInterpolation(temp[ceil_y * w + floor_x].red, temp[ceil_y * w + ceil_x].red, 1-fraction_x);
				((argb*)pixelscolor)[id].red = LinearInterpolation(p1,p2,one_minus_y);

			}
		}
	}
	//LOGI("smooth xong");
	free(temp);
	return 0;
}
int ImageSmoothAbsApply(void **pointerpixelscolor, Point *effected, Point *pt, int n, int w, int h, int stride)
{
	void* pixelscolor = *pointerpixelscolor;
	argb *temp = (argb *)malloc(w*h*sizeof(argb));
	int x, y;
	// clone pixelcolor into a array
	argb * line = (argb *) pixelscolor;
	for (y=0;y<h;y++) {
	    	for (x=0;x<w;x++) {
	    		int id = y * w + x;
	    		temp[id].red = line[id].red;
	    		temp[id].green = line[id].green;
	    		temp[id].blue = line[id].blue;
	    	}
	    }

	//===========================================================================================
	// smooth
	int newX, newY;
	y = 0;
	//LOGI("%d %d", y, h);
	int i;
	for(i = 0; i<n ;i++)
	{
		x = effected[i].X;
		y = effected[i].Y;

		newX = pt[i].X;
		newY = pt[i].Y;

		// Setup
		int id = y * w + x;
		int newid = newY *w + newX;
		((argb*)pixelscolor)[id].red = temp[newid].red;
		((argb*)pixelscolor)[id].blue = temp[newid].blue;
		((argb*)pixelscolor)[id].green = temp[newid].green;
	}
	//LOGI("smooth xong");
	free(temp);
	return 0;
}

int Warper_Swirl(JNIEnv* env, jobject thiz, jobject bitmap, int pointx, int pointy, int area, int intensity, int rotate_direction, int smooth)
{
    AndroidBitmapInfo  infocolor;
    void*              pixelscolor;
    int                ret;

    LOGI("Swirl");
	if ((ret = AndroidBitmap_getInfo(env, bitmap, &infocolor)) < 0) {
		LOGE("AndroidBitmap_getInfo() failed ! error=%d", ret);
		return 1;
    }
    if ((ret = AndroidBitmap_lockPixels(env, bitmap, &pixelscolor)) < 0) {
        LOGE("AndroidBitmap_lockPixels() failed ! error=%d", ret);
        return 2;
    }

	int h = infocolor.height;
	int w = infocolor.width;

	double theta;
	double newX, newY;

	float intensity_new = intensity / 600.0f;
	int radius = area / 2;
	int radius_scan = (int)(radius * sqrt(2));

	FloatPoint *fp = (FloatPoint*)malloc(h * w * sizeof(FloatPoint));
	Point *pt = (Point*)malloc(h * w * sizeof(Point));
	Point *effected_point = (Point*)malloc(h * w * sizeof(Point));

	int x,y;
	//LOGI("midX = %d, midY = %d", pointx, pointy);
	int i, j;
	int count = 0;
	for(i = - radius_scan; i <= radius_scan; i++)
	{
		for(j = -radius_scan; j<= radius_scan; j++)
		{
			x = pointx + i;
			y = pointy + j;

			if(x < 0 || x >= w)
				continue;
			if(y<0 || y >= h)
				continue;

			float local_radius = sqrt(i*i + j*j);
			if(local_radius > radius)
				continue;
			theta = atan2(j, i);
			//LOGI("angel = %8.3f, radius = %8.3f, intensity = %d", theta, local_radius, intensity_new);
			float driftX = local_radius * cos(theta + intensity_new * (radius - local_radius) * rotate_direction);
			float driftY = local_radius * sin(theta + intensity_new * (radius - local_radius) * rotate_direction);

			newX = pointx + driftX;
			newY = pointy + driftY;


			int id = y * w + x;

			if(newX >=0 && newX < w && newY >=0 && newY < h)
			{
				fp[count].X = newX;
				pt[count].X = round(newX);

				fp[count].Y = newY;
				pt[count].Y = round(newY);

				effected_point[count].X = x;
				effected_point[count].Y = y;
				count++;
			}
			//LOGI("%d %d %8.3f %8.3f %8.3f %8.3f", x, y, driftX, driftY, fp[id].X, fp[id].Y);
		}
	}
	if(smooth == 1)
		ImageSmoothApply(&pixelscolor, effected_point, fp, count, w, h, infocolor.stride);
	else
		ImageSmoothAbsApply(&pixelscolor, effected_point, pt, count, w, h, infocolor.stride);

	free(fp);
	free(pt);
	free(effected_point);
	AndroidBitmap_unlockPixels(env, bitmap);

	return 3;
}
int Warper_SphereOut(JNIEnv* env, jobject thiz, jobject bitmap, jint pointx, jint pointy, jint area, jint intensity, int smooth)
{
    AndroidBitmapInfo  infocolor;
    void*              pixelscolor;
    int                ret;

    LOGI("Sphere out");
	if ((ret = AndroidBitmap_getInfo(env, bitmap, &infocolor)) < 0) {
		LOGE("AndroidBitmap_getInfo() failed ! error=%d", ret);
		return 1;
    }
    if ((ret = AndroidBitmap_lockPixels(env, bitmap, &pixelscolor)) < 0) {
        LOGE("AndroidBitmap_lockPixels() failed ! error=%d", ret);
        return 2;
    }

	int h = infocolor.height;
	int w = infocolor.width;

	double theta;
	double newX, newY;

	int radius = area / 2;
	int radius_scan = (int)(radius * sqrt(2));
	FloatPoint *fp = (FloatPoint*)malloc(h * w * sizeof(FloatPoint));
	Point *pt = (Point*)malloc(h * w * sizeof(Point));
	Point *effected_point = (Point*)malloc(h * w * sizeof(Point));

	int x,y;

	int i,j;
	int count = 0;
	for(i = - radius_scan; i <= radius_scan;i++)
	{
		for(j = -radius_scan; j<= radius_scan; j++)
		{
			x = pointx + i;
			y = pointy + j;

			if(x < 0 || x >= w)
				continue;
			if(y<0 || y >= h)
				continue;

			float local_radius = sqrt(i*i + j*j);
			if(local_radius > radius)
				continue;
			theta = atan2(j, i);
		
			double newRadius = local_radius * (local_radius)/radius;
			//double newRadius = local_radius - (radius - local_radius) * (intensity / 100.0f);
			//newRadius = newRadius + (radius - newRadius) * (intensity / 100.0f);

			//LOGI("old = %8.3f, new = %8.3f", local_radius, newRadius);

			newX = pointx + (newRadius * cos(theta));
			newY = pointy + (newRadius * sin(theta));

			int id = y * w + x;

			if(newX >=0 && newX < w && newY >=0 && newY < h)
			{
				fp[count].X = newX;
				pt[count].X = round(newX);

				fp[count].Y = newY;
				pt[count].Y = round(newY);

				effected_point[count].X = x;
				effected_point[count].Y = y;
				count++;
			}
		}
	}
	if (smooth == 1)
		ImageSmoothApply(&pixelscolor, effected_point, fp, count, w, h, infocolor.stride);
	else
		ImageSmoothAbsApply(&pixelscolor, effected_point, pt, count, w, h, infocolor.stride);

	free(fp);
	free(pt);
	free(effected_point);
	AndroidBitmap_unlockPixels(env, bitmap);

	return 3;
}
int Warper_SphereIn(JNIEnv* env, jobject thiz, jobject bitmap, jint pointx, jint pointy, jint area, jint intensity, int smooth)
{
    AndroidBitmapInfo  infocolor;
    void*              pixelscolor;
    int                ret;

    LOGI("convertToGray");
	if ((ret = AndroidBitmap_getInfo(env, bitmap, &infocolor)) < 0) {
		LOGE("AndroidBitmap_getInfo() failed ! error=%d", ret);
		return 1;
    }
    if ((ret = AndroidBitmap_lockPixels(env, bitmap, &pixelscolor)) < 0) {
        LOGE("AndroidBitmap_lockPixels() failed ! error=%d", ret);
        return 2;
    }

	int h = infocolor.height;
	int w = infocolor.width;

	double theta;
	double newX, newY;

	int radius = area / 2;
	int radius_scan = (int)(radius * sqrt(2));
	FloatPoint *fp = (FloatPoint*)malloc(h * w * sizeof(FloatPoint));
	Point *pt = (Point*)malloc(h * w * sizeof(Point));
	Point *effected_point = (Point*)malloc(w*h*sizeof(Point));

	int count = 0;
	int i, j;
	int x, y;
	for(i = - radius_scan; i <= radius_scan;i++)
	{
		for(j = -radius_scan; j<= radius_scan; j++)
		{
			x = pointx + i;
			y = pointy + j;

			if(x < 0 || x >= w)
				continue;
			if(y<0 || y >= h)
				continue;

			float local_radius = sqrt(i*i + j*j);
			if(local_radius > radius)
				continue;
			theta = atan2(j, i);

			//double newRadius = local_radius * (local_radius)/radius;
			//double newRadius = sqrt(local_radius) * (intensity);
			double newRadius = local_radius + (radius - local_radius) * (intensity / 100.0f);

			//LOGI("old = %8.3f, new = %8.3f", local_radius, newRadius);

			newX = pointx + (newRadius * cos(theta));
			newY = pointy + (newRadius * sin(theta));

			int id = y * w + x;

			if(newX >=0 && newX < w && newY >=0 && newY < h)
			{
				fp[count].X = newX;
				pt[count].X = round(newX);

				fp[count].Y = newY;
				pt[count].Y = round(newY);

				effected_point[count].X = x;
				effected_point[count].Y = y;
				count++;
			}
		}
	}
	if (smooth == 1)
		ImageSmoothApply(&pixelscolor, effected_point, fp, count, w, h, infocolor.stride);
	else
		ImageSmoothAbsApply(&pixelscolor, effected_point, pt, count, w, h, infocolor.stride);

	free(fp);
	free(pt);
	free(effected_point);
	AndroidBitmap_unlockPixels(env, bitmap);

	return 3;
}
int Warper_Move(JNIEnv* env, jobject thiz, jobject bitmap, jint pointx, jint pointy, jint area, jint intensity, jint endx, jint endy, int smooth)
{
	// kiem tra do lon vector di chuyen co khac 0 hay khong
	if(pointx == endx && pointy == endy)
	{
		return 1;
	}

    AndroidBitmapInfo  infocolor;
    void*              pixelscolor;
    int                ret;

    LOGI("convertToGray");
	if ((ret = AndroidBitmap_getInfo(env, bitmap, &infocolor)) < 0) {
			LOGE("AndroidBitmap_getInfo() failed ! error=%d", ret);
			return 2;
    }
    if ((ret = AndroidBitmap_lockPixels(env, bitmap, &pixelscolor)) < 0) {
        LOGE("AndroidBitmap_lockPixels() failed ! error=%d", ret);
        return 3;
    }

	int h = infocolor.height;
	int w = infocolor.width;

	double theta;

	int radius = area / 2;

	// mang chua cac diem anh can xu ly move
	FloatPoint *fp = (FloatPoint*)malloc(h * w * sizeof(FloatPoint));
	Point *pt = (Point*)malloc(h * w * sizeof(Point));
	Point *effected_point = (Point*)malloc(w*h*sizeof(Point));
	float *discount = (float*)malloc(h*w*sizeof(float));

	//tinh toan huong di chuyen
	float deltaX = endx - pointx;
	float deltaY = endy - pointy;
	double magnitude = sqrt(deltaX * deltaX + deltaY * deltaY);		// do lon vector
	float vector_X = (float)(deltaX / magnitude);		// chuan hoa vector di chuyen
	float vector_Y = (float)(deltaY / magnitude);
	float effected_distance = (float)magnitude;// * (intensity / 50.0f);

	//------------------------------------------------------------------------------------
	// Tinh toan pham vi anh huong
	// ==> Tim 2 duong vuong goc voi huong di chuyen tai diem dau va cuoi
	// tat ca nhung diem noi suy tuyen tinh giua 2 duong nay deu thuoc tam anh huong
	Point *startline = (Point*)malloc((2*radius + 1)*sizeof(Point));
	Point *endline = (Point*)malloc((2*radius + 1)*sizeof(Point));

	// phuong trinh duong thang qua 2 diem dau đầu ==> d1
	float _a1 = -vector_Y;
	float _b1 = vector_X;
	float _c1 = -(_a1 * pointx + _b1*pointy);
	float _d1 = (float)sqrt(_a1 * _a1 + _b1 * _b1);

	// phuong trinh duong thang qua diem dau va vuong voi duong d1 == d2
	float _a2 = vector_X;
	float _b2 = vector_Y;
	float _c2 = -(_a2 * pointx + _b2*pointy);
	float _d2 = (float)sqrt(_a2*_a2 + _b2*_b2);

	// phuong trinh duong thang qua diem cuoi va vuong voi duong d1 == d3
	float _a3 = vector_X;
	float _b3 = vector_Y;
	float _c3 = -(_a3 * endx + _b3*endy);
	float _d3 = (float)sqrt(_a3*_a3 + _b3*_b3);

	// Xem diem cuoi la diem apply effect ==> effect tu diem (endx, endy) toi (endx_2, endy_2)
	float endX_2 = endx;// + effected_distance * vector_X;
	float endY_2 = endy;// + effected_distance * vector_Y;

	LOGI("effected distance = %5.2f --> (%d, %d) (%5.2f, %5.2f)", effected_distance, pointx, pointy, endx, endy);

	int x,y;
	int i,j;
	float drift_X, drift_Y;

	int id = 0;
	float local_distance, end_distance;
	float newX, newY;
	int ii, jj;
	float main_distance = (float)magnitude;
	float main_distance_devive_b1y_2 = main_distance / 2;
	float sigma = (radius / 1.0f);

	// Tinh toan do dich chuyen tung pixel theo 1 day chuan
	float max_discount = radius;
	for(i= - radius; i<= 0; i++)
	{
		ii = i + radius;
		// do dich chuyen theo phan phoi normal
		discount[ii] = (float)exp(-(ii)*(ii) / (2 * sigma * sigma));
		//discount[ii] = (float)ii * ii / (radius * radius);
		if(max_discount > discount[ii])
			max_discount = discount[ii];
		//LOGI("i = %d --> %5.2f", i, discount[ii]);
	}
	for(i= - radius; i<= 0; i++)
	{
		ii = i + radius;
		discount[ii] = discount[ii] - max_discount;		// chuan hoa vector discount
		LOGI("i = %d --> %5.2f", i, discount[ii]);
		if(i != 0)
		{
			discount[area - ii] = discount[ii];
		}
	}

	// Xac dinh vung anh huong cua cac pixel
	int x_min, x_max, y_min, y_max;
	if(pointx < endx)
	{
		x_min = pointx - radius;
		x_max = endx + radius;
	}
	else
	{
		x_min = endx - radius;
		x_max = pointx + radius;
	}
	if(pointy < endy)
	{
		y_min = pointy - radius;
		y_max = endy + radius;
	}
	else
	{
		y_min = endy - radius;
		y_max = pointy + radius;
	}

	// Gan mang vi tri moi cho tung pixel
	float l = effected_distance / 2;
	for(x = x_min; x <= x_max; x++)
	{
		if(x < 0 || x >= w)
			continue;
		for(y = y_min; y<y_max ; y++)
		{
			if(y <0 || y >= h)
				continue;

			float dis_d1 = Distance_toline(x, y, _a1, _b1, _c1, _d1);

			//LOGI("(%d, %d) (%d, %d) (%5.2f, %5.2f)--> distance = %5.2f", x, y, endx, endy, endX_2, endY_2, dis_d1);
			if(dis_d1 > radius)
				continue;

			//float local_distance1 = Distance(x, y, endx, endy);
			//float local_distance2 = Distance(x, y, endX_2, endY_2);
			//if(local_distance1 >= radius && local_distance2 >= radius)
				//continue;

			end_distance = 2*radius;
			if((_a2*x + _b2*y + _c2)*(_a3*x + _b3*y + _c3) > 0)
			{
				end_distance = Distance(x, y, endx, endy);
				if(end_distance > radius)
					continue;
			}

			float dis_d2 = Distance_toline(x, y, _a2, _b2, _c2, _d2);
			float dis = dis_d2 / 4;

			// diem bi anh huong
			effected_point[id].X = x;
			effected_point[id].Y = y;

			// vi tri mau moi cua diem anh huong
			float temp;
			if(end_distance < radius / 2 || end_distance == 2 * radius)
				temp =  - discount[(int)(dis_d1)] * dis;
			else
				temp = - discount[(int)(dis_d1)] * dis * (radius - end_distance) / radius;
			drift_X = (temp) * vector_X;
			drift_Y = (temp) * vector_Y;

			newX = x + drift_X;
			newY = y + drift_Y;
			fp[id].X = newX;
			fp[id].Y = newY;
			pt[id].X = round(newX);
			pt[id].Y = round(newY);
			id++;

			//LOGI("(%d, %d) + drift (%5.2f, %5.2f) --> (%5.2f, %5.2f)", x, y, drift_X, drift_Y, newX, newY);
		}
	}
	int count = id;
	if (smooth == 1)
		ImageSmoothApply(&pixelscolor, effected_point, fp, count, w, h, infocolor.stride);
	else
		ImageSmoothAbsApply(&pixelscolor, effected_point, pt, count, w, h, infocolor.stride);

	//------------------------------------------------------------------------------------

	free(pt);
	free(effected_point);
	free(fp);
	free(discount);
	free(startline);
	free(endline);
	AndroidBitmap_unlockPixels(env, bitmap);

	return 4;
}
int Warper_Move_b1K(JNIEnv* env, jobject thiz, jobject bitmap, jint pointx, jint pointy, jint area, jint intensity, jint endx, jint endy)
{
	// kiem tra do lon vector di chuyen co khac 0 hay khong
	if(pointx == endx && pointy == endy)
	{
		return 1;
	}

    AndroidBitmapInfo  infocolor;
    void*              pixelscolor;
    int                ret;

    LOGI("convertToGray");
	if ((ret = AndroidBitmap_getInfo(env, bitmap, &infocolor)) < 0) {
			LOGE("AndroidBitmap_getInfo() failed ! error=%d", ret);
			return 2;
    }
    if ((ret = AndroidBitmap_lockPixels(env, bitmap, &pixelscolor)) < 0) {
        LOGE("AndroidBitmap_lockPixels() failed ! error=%d", ret);
        return 3;
    }

	int h = infocolor.height;
	int w = infocolor.width;

	double theta;
	double newX, newY;

	int radius = area / 2;

	// mang chua cac diem anh can xu ly move
	Point *pt = (Point*)malloc(h*w * sizeof(Point));
	float *discount = (float*)malloc(h*w*sizeof(float));
	int x,y;

	for(y = 0; y < h ; y++ )
	{
		for(x = 0;x < w ; x ++)
		{
			int id = y * w + x;
			pt[id].X = -1;
			pt[id].Y = -1;
		}
	}

	//tinh toan huong di chuyen
	float deltaX = endx - pointx;
	float deltaY = endy - pointy;
	double magnitude = sqrt(deltaX * deltaX + deltaY * deltaY);		// do lon vector
	float vector_X = (float)(deltaX / magnitude);		// chuan hoa vector di chuyen
	float vector_Y = (float)(deltaY / magnitude);

	//------------------------------------------------------------------------------------
	// Tinh toan pham vi anh huong
	// ==> Tim 2 duong vuong goc voi huong di chuyen tai diem dau va cuoi
	// tat ca nhung diem noi suy tuyen tinh giua 2 duong nay deu thuoc tam anh huong
	Point *startline = (Point*)malloc((2*radius + 1)*sizeof(Point));
	Point *endline = (Point*)malloc((2*radius + 1)*sizeof(Point));
	float _a1 = -vector_Y;
	float _b1 = vector_X;
	float _c = - (_a1 * pointx + _b1*pointy);

	int i,j;
	for(i = -radius; i<= radius; i++)
	{
		startline[j].X = pointx + (int)(i*_a1);
		startline[j].Y = pointy + (int)(i*_b1);

		endline[j].X = endx + (int)(i*_a1);
		endline[j].Y = endy + (int)(i*_b1);
		j++;
	}
	int id = 0;
	for(i=0; i< 2*radius + 1; i++)
	{
		float x = startline[i].X;
		float y = startline[i].Y;
		float main_distance = sqrt((startline[i].X - endline[i].X)*(startline[i].X - endline[i].X) + (startline[i].Y - endline[i].Y)*(startline[i].Y - endline[i].Y));

		while(Distance(x,y,startline[i].X, startline[i].Y) < main_distance)
		{
			pt[id].X = x;
			pt[id].Y = y;
			discount[id] = i - radius;
			id++;

			x = x + vector_X;
			y = y + vector_Y;
		}
	}
	int point_radius = radius * sqrt(2);
	for(i = -point_radius; i<= point_radius; i++)
	{
		for(j = -point_radius; j<= point_radius; j++)
		{
			if (sqrt(i*i + j* j) > radius)
				continue;
			pt[id].X = endx + i;
			pt[id].Y = endy + j;
			discount[id] = 0;
			id++;
		}
	}
	int n = id;
	//------------------------------------------------------------------------------------

	// Tinh toan ma tran filter cho viec motion blur
	int egde = 9;
	float *matrix = (float*)malloc(egde*egde*sizeof(float));
	for(i = 0;i<egde*egde;i++)
	{
		matrix[i] = 0;
	}
	matrix[(egde/2)*egde + (egde/2)] = 1;
	float t = radius + egde/2 + 1;
	int mid = egde/2;
	for(i = 0; i<egde/2;i++)
	{
		int idx = (int)(mid + i*(-vector_X) + 0.5f);
		int idy = (int)(mid + i*(-vector_Y) + 0.5f);

		matrix[idy * egde + idx] = t;
		t -= 1;
	}
	float sum_matrix = 0;
	for(i =0; i<egde*egde; i++)
	{
		//LOGI("%d: %8.3f", i, matrix[i]);
		sum_matrix += matrix[i];
	}
	//LOGI("sum %8.3f", sum_matrix);
	// Clone image
	argb *temp = (argb *)malloc(w*h*sizeof(argb));
	argb * line = (argb *) pixelscolor;
	for (y=0;y<h;y++) {
	    	for (x=0;x<w;x++) {
	    		int id = y * w + x;
	    		temp[id].red = line[id].red;
	    		temp[id].green = line[id].green;
	    		temp[id].blue = line[id].blue;
	    	}
	    }

	// Apply filter len nhung diem can xu ly
	for(id = 0;id<n;id++)
	{
		int x = pt[id].X;
		int y = pt[id].Y;

		float red = 0;
		float green = 0;
		float blue = 0;
		int disc = discount[id];
		if (disc > 0)
			disc = -disc;
		float sum = 0;
		for(i = -egde/2; i<=egde/2; i++)
		{
			for(j = -egde/2; j <= egde/2; j++)
			{
				int _x = x + i;
				int _y = y + j;
				if(_x < 0 && _x >= w && _y <0 && _y >= h)
					continue;
				int k = _y * w + _x;
				int l = (j + egde/2) * egde + (i + egde/2);
				float matrix_value = matrix[l];
				if(matrix_value != 0)
					matrix_value += disc;
				sum += matrix_value;
				red = red + temp[k].red * matrix_value;
				green = green + temp[k].green * matrix_value;
				blue = blue + temp[k].blue * matrix_value;

				//LOGI("x = %d, y = %d, i = %d, j = %d, red %8.3f, drift = %8.3f", _x, _y , i /2, j, red, matrix_value);
			}
		}

		line[y*w + x].red = (uint8_t)(red / sum);
		line[y*w + x].green = (uint8_t)(green / sum);
		line[y*w + x].blue = (uint8_t)(blue / sum);
		//LOGI("red %8.3f green %8.3f blue %8.3f , %d %d %d", red, green, blue, line[y*w + x].red, line[y*w + x].green, line[y*w + x].blue);
	}

	free(pt);
	free(discount);
	free(temp);
	free(matrix);
	free(startline);
	free(endline);
	AndroidBitmap_unlockPixels(env, bitmap);

	return 4;
}
int Warper_Undo(JNIEnv* env, jobject thiz, jobject originbitmap, jobject bitmap, jint pointx, jint pointy, jint area, jint intensity)
{
    AndroidBitmapInfo  infocolor;
    void*              pixelscolor;
    AndroidBitmapInfo  info_origin;
    void*              pixel_origin;
    int                ret;

    //LOGI("convertToGray");
	if ((ret = AndroidBitmap_getInfo(env, bitmap, &infocolor)) < 0) {
			LOGE("AndroidBitmap_getInfo() failed ! error=%d", ret);
			return 1;
    }
    if ((ret = AndroidBitmap_lockPixels(env, bitmap, &pixelscolor)) < 0) {
        LOGE("AndroidBitmap_lockPixels() failed ! error=%d", ret);
        return 2;
    }

    //LOGI("convertToGray");
	if ((ret = AndroidBitmap_getInfo(env, originbitmap, &info_origin)) < 0) {
			LOGE("AndroidBitmap_getInfo() failed ! error=%d", ret);
			return 3;
    }
    if ((ret = AndroidBitmap_lockPixels(env, originbitmap, &pixel_origin)) < 0) {
        LOGE("AndroidBitmap_lockPixels() failed ! error=%d", ret);
        return 4;
    }

    // So sanh kich thuoc so voi anh goc
    if(infocolor.height != info_origin.height || infocolor.width != info_origin.width)
    {
    	AndroidBitmap_unlockPixels(env, bitmap);
    	AndroidBitmap_unlockPixels(env, originbitmap);
    	return 5;
    }

    int h = infocolor.height;
	int w = infocolor.width;
	int radius = area / 2;
	float fraction = (float)(intensity + 50)/100;
	if(fraction > 1)
		fraction = 1;

	argb *line = (argb*)pixelscolor;
	argb *line_origin = (argb*)pixel_origin;

	int i,j;
	for(i = - radius; i < radius;i++)
	{
		for(j = -radius; j< radius; j++)
		{
			int x = pointx + i;
			int y = pointy + j;

			if(x < 0 || x >= w)
				continue;
			if(y<0 || y >= h)
				continue;

			float local_radius = i*i + j*j;
			if(local_radius > radius * radius)
				continue;

			int id = y * w + x;
			line[id].red = (uint8_t)( (1 - fraction) * line[id].red + fraction * line_origin[id].red);
			line[id].green = (uint8_t)( (1 - fraction) * line[id].green + fraction * line_origin[id].green);
			line[id].blue = (uint8_t)( (1 - fraction) * line[id].blue + fraction * line_origin[id].blue);
		}
	}

	AndroidBitmap_unlockPixels(env, bitmap);
	AndroidBitmap_unlockPixels(env, originbitmap);

	return 6;
}
int Warper_Create_Swirl_Matrix(JNIEnv* env, jobject thiz, jfloatArray  matrixX, jfloatArray  matrixY, jint n, jint w, jint h, jint intensity, int rotate_direction)
{
	double theta;
	double newX, newY;
	float changeVectorX, changeVectorY;

	//LOGI("start create, area %d, w %d, h %d, intensity %d", n, w, h, intensity);
	//jfloat *matrix_X = (jfloat*)matrixX;
	//jfloat *matrix_Y = (jfloat*)matrixY;

	//LOGI("initial");
	jfloat *matrix_X = (*env)->GetFloatArrayElements(env, matrixX, 0);
	jfloat *matrix_Y = (*env)->GetFloatArrayElements(env, matrixY, 0);
	//LOGI("initial");
	float intensity_new = intensity / 600.0f;
	int radius = n / 2;

	int x,y;
	int i, j;
	int index;
	for(j = -radius; j<= 0; j++)
	{
		for(i = - radius; i < radius; i++)
		{
			float local_radius = sqrt(i*i + j*j);
			changeVectorX = 0;
			changeVectorY = 0;

			if(local_radius < radius)
			{
				theta = atan2(j, i);
				//LOGI("angel = %8.3f, radius = %8.3f, intensity = %d", theta, local_radius, intensity_new);
				float driftX = local_radius * cos(theta + intensity_new * (radius - local_radius) * rotate_direction);
				float driftY = local_radius * sin(theta + intensity_new * (radius - local_radius) * rotate_direction);

				newX = driftX;
				newY = driftY;

				changeVectorX = newX - i;
				changeVectorY = newY - j;

			}

			// value for (-x,-y)
			index = (j + radius) * n + (i + radius);
			matrix_X[index] = changeVectorX;
			matrix_Y[index] = changeVectorY;
			//LOGI("id = %d ; (%d, %d) --> vector(%5.3f, %5.3f)", index, i + radius, j + radius, matrix_X[index], matrix_Y[index]);

			//value for (x, y)
			index = (-j + radius) * n + (-i + radius);
			matrix_X[index] = -changeVectorX;
			matrix_Y[index] = -changeVectorY;
			//LOGI("id = %d ; (%d, %d) --> vector(%5.3f, %5.3f)", index, -i + radius, -j + radius, matrix_X[index], matrix_Y[index]);
		}
	}
	(*env)->ReleaseFloatArrayElements(env, matrixX, matrix_X, 0);
	(*env)->ReleaseFloatArrayElements(env, matrixY, matrix_Y, 0);
	//LOGI("end create");
	return 3;
}
int Warper_Create_SphereIn_Matrix(JNIEnv* env, jobject thiz, jfloatArray  matrixX, jfloatArray  matrixY, jint n, jint w, jint h, jint intensity)
{
	double theta;
	double newX, newY;
	float changeVectorX, changeVectorY;

	//LOGI("start create, area %d, w %d, h %d, intensity %d", n, w, h, intensity);
	//LOGI("initial");
	jfloat *matrix_X = (*env)->GetFloatArrayElements(env, matrixX, 0);
	jfloat *matrix_Y = (*env)->GetFloatArrayElements(env, matrixY, 0);
	//LOGI("initial");
	int radius = n / 2;

	int x,y;
	int i, j;
	int index;
	for(j = -radius; j<= 0; j++)
	{
		for(i = - radius; i < radius; i++)
		{
			float local_radius = sqrt(i*i + j*j);
			changeVectorX = 0;
			changeVectorY = 0;

			if(local_radius < radius)
			{
				theta = atan2(j, i);
				float newRadius = local_radius + (radius - local_radius) * (intensity / 100.0f);
				//LOGI("angel = %8.3f, radius = %8.3f, intensity = %d", theta, local_radius, intensity_new);

				newX = newRadius * cos(theta);
				newY = newRadius * sin(theta);

				changeVectorX = newX - i;
				changeVectorY = newY - j;

			}

			// value for (-x,-y)
			index = (j + radius) * n + (i + radius);
			matrix_X[index] = changeVectorX;
			matrix_Y[index] = changeVectorY;
			//LOGI("id = %d ; (%d, %d) --> vector(%5.3f, %5.3f)", index, i + radius, j + radius, matrix_X[index], matrix_Y[index]);

			//value for (x, y)
			index = (-j + radius) * n + (-i + radius);
			matrix_X[index] = -changeVectorX;
			matrix_Y[index] = -changeVectorY;
			//LOGI("id = %d ; (%d, %d) --> vector(%5.3f, %5.3f)", index, -i + radius, -j + radius, matrix_X[index], matrix_Y[index]);
		}
	}
	(*env)->ReleaseFloatArrayElements(env, matrixX, matrix_X, 0);
	(*env)->ReleaseFloatArrayElements(env, matrixY, matrix_Y, 0);
	//LOGI("end create");
	return 3;
}
int Warper_Create_SphereOut_Matrix(JNIEnv* env, jobject thiz, jfloatArray  matrixX, jfloatArray  matrixY, jint n, jint w, jint h, jint intensity)
{
	double theta;
	double newX, newY;
	float changeVectorX, changeVectorY;

	//LOGI("start create, area %d, w %d, h %d, intensity %d", n, w, h, intensity);
	//LOGI("initial");
	jfloat *matrix_X = (*env)->GetFloatArrayElements(env, matrixX, 0);
	jfloat *matrix_Y = (*env)->GetFloatArrayElements(env, matrixY, 0);
	//LOGI("initial");
	int radius = n / 2;

	int x,y;
	int i, j;
	int index;
	for(j = -radius; j<= 0; j++)
	{
		for(i = - radius; i < radius; i++)
		{
			float local_radius = sqrt(i*i + j*j);
			changeVectorX = 0;
			changeVectorY = 0;

			if(local_radius < radius)
			{
				theta = atan2(j, i);
				float newRadius = local_radius * (local_radius)/radius;
				//LOGI("angel = %8.3f, radius = %8.3f, intensity = %d", theta, local_radius, intensity_new);

				newX = newRadius * cos(theta);
				newY = newRadius * sin(theta);

				changeVectorX = newX - i;
				changeVectorY = newY - j;

			}

			// value for (-x,-y)
			index = (j + radius) * n + (i + radius);
			matrix_X[index] = changeVectorX;
			matrix_Y[index] = changeVectorY;
			//LOGI("id = %d ; (%d, %d) --> vector(%5.3f, %5.3f)", index, i + radius, j + radius, matrix_X[index], matrix_Y[index]);

			//value for (x, y)
			index = (-j + radius) * n + (-i + radius);
			matrix_X[index] = -changeVectorX;
			matrix_Y[index] = -changeVectorY;
			//LOGI("id = %d ; (%d, %d) --> vector(%5.3f, %5.3f)", index, -i + radius, -j + radius, matrix_X[index], matrix_Y[index]);
		}
	}
	(*env)->ReleaseFloatArrayElements(env, matrixX, matrix_X, 0);
	(*env)->ReleaseFloatArrayElements(env, matrixY, matrix_Y, 0);
	//LOGI("end create");
	return 3;
}
int Warper_Transform(JNIEnv* env, jobject thiz, jobject bitmap, jfloatArray matrixX, jfloatArray matrixY, jint pointx, jint pointy, jint area, jint intensity, int smooth)
{
    AndroidBitmapInfo  infocolor;
    void*              pixelscolor;
    int                ret;

    LOGI("convertToGray");
	if ((ret = AndroidBitmap_getInfo(env, bitmap, &infocolor)) < 0) {
		LOGE("AndroidBitmap_getInfo() failed ! error=%d", ret);
		return 1;
    }
    if ((ret = AndroidBitmap_lockPixels(env, bitmap, &pixelscolor)) < 0) {
        LOGE("AndroidBitmap_lockPixels() failed ! error=%d", ret);
        return 2;
    }

	jfloat *matrix_X = (*env)->GetFloatArrayElements(env, matrixX, 0);
	jfloat *matrix_Y = (*env)->GetFloatArrayElements(env, matrixY, 0);

	int h = infocolor.height;
	int w = infocolor.width;

	double theta;
	double newX, newY;

	int radius = area / 2;
	FloatPoint *fp = (FloatPoint*)malloc(h * w * sizeof(FloatPoint));
	Point *pt = (Point*)malloc(h * w * sizeof(Point));
	Point *effected_point = (Point*)malloc(w*h*sizeof(Point));

	int count = 0;
	int i, j;
	int x, y;
	int index = 0;
	for(i = - radius; i <= radius;i++)
	{
		for(j = -radius; j<= radius; j++)
		{
			x = pointx + i;
			y = pointy + j;

			if(x < 0 || x >= w)
				continue;
			if(y<0 || y >= h)
				continue;

			index = (j + radius) * area + (i + radius);
			newX = x + matrix_X[index];
			newY = y + matrix_Y[index];

			if(newX >=0 && newX < w && newY >=0 && newY < h)
			{
				fp[count].X = newX;
				pt[count].X = round(newX);

				fp[count].Y = newY;
				pt[count].Y = round(newY);

				effected_point[count].X = x;
				effected_point[count].Y = y;
				count++;

				//LOGI("(%d, %d) --> (%5.3f, %5.3f)", x, y, newX, newY);
			}
		}
	}
	if (smooth == 1)
		ImageSmoothApply(&pixelscolor, effected_point, fp, count, w, h, infocolor.stride);
	else
		ImageSmoothAbsApply(&pixelscolor, effected_point, pt, count, w, h, infocolor.stride);

	free(fp);
	free(pt);
	free(effected_point);
	AndroidBitmap_unlockPixels(env, bitmap);
	(*env)->ReleaseFloatArrayElements(env, matrixX, matrix_X, 0);
	(*env)->ReleaseFloatArrayElements(env, matrixY, matrix_Y, 0);
	return 3;
}
